#! /usr/bin/env python3

# import rospy
# import tf2_ros
# from geometry_msgs.msg import TransformStamped
# from tf2_geometry_msgs import PointStamped





# if __name__ == "__main__":
#     rospy.init_node("SCTransfor_Sub")
#     buffer =tf2_ros.Buffer()
#     listener=tf2_ros.TransformListener(buffer)
#     rate = rospy.Rate(1)
#     while not rospy.is_shutdown():
#         point_source=PointStamped()
#         point_source.header.frame_id="laser"
#         point_source.header.stamp=rospy.Time.now()
#         point_source.point.x=10
#         point_source.point.y=2
#         point_source.point.z=3

#         try:
#             point_targe=buffer.transform(point_source,"world")
#             print("转换结果:x=%.2f,y=%.2f,z=%.2f",point_targe.point.x,point_targe.point.y,point_targe.point.z)
#         except (tf2_ros.LookupException,tf2_ros.ConnectivityException,tf2_ros.ExtrapolationException) as e:
#             rospy.logerr("异常:%s",e)
#         rate.sleep()


    # 1.导包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("SCTransfor_Sub")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer(cache_time=rospy.Duration(10))
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 4.创建一个 radar 坐标系中的坐标点
        point_source = PointStamped()
        point_source.header.frame_id = "radar"
        point_source.header.stamp = rospy.Time(0)
        point_source.point.x = 10
        point_source.point.y = 2
        point_source.point.z = 3

        try:
    #     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            # 确保目标坐标系 "world" 已经被广播
            if buffer.can_transform(point_source.header.frame_id, "world", rospy.Time(0)):
                point_target = buffer.transform(point_source, "world")
                rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                                point_target.point.x,
                                point_target.point.y,
                                point_target.point.z)
            else:
                rospy.logwarn("目标坐标系 'world' 不存在")
        except Exception as e:
            rospy.logerr("异常:%s", e)

    #     6.spin
        rate.sleep()